package cas;

import gov.nasa.worldwind.geom.LatLon;

import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
import java.util.Observable;

import util.Converter;
import util.LengthMeasurer;
import util.NumberUtil;
import util.RuteUtil;
import bean.KoordinatBean;

public class CAS extends Observable{

	private Identity myIdentity;
	private List<Identity> anotherTrains;
	private Radio radio;
	private GPSSimulator gpsSimulator;
	private Integer[] sameId;
	private String sendItem;
	private String receiveItem;
	private double distance;
	private boolean atFront;
	private boolean inSameDirection;
	private Message message;
	private boolean collide;
	private Thread gpsThread;
	private Integer[] listCoordId;
	public double timeToStop;
	public double timeToSafeDistance;
	public CAS() {
		anotherTrains = new ArrayList<Identity>();
		collide = false;
		atFront = false;
		inSameDirection = false;
	}

	public void setParameter(String radioPortName,GPSSimulator gpsSimulator){
		this.gpsSimulator = gpsSimulator;
		myIdentity = new Identity();
		myIdentity = this.gpsSimulator.getMyIdentity().copy();
		radio = new Radio(radioPortName);
		gpsThread = new Thread(gpsSimulator);
		gpsThread.start();
	}

	public void start(){
		try{
			myIdentity.setTimeDiff(gpsSimulator.getSpeed().getTimeDiff());
			myIdentity.setCurCoord(gpsSimulator.getCurCoordinate());
			myIdentity.setUrutan(gpsSimulator.getMyIdentity().getUrutan());
			if(radio.getSerialPort().isDSR()) {
				radio.send(myIdentity);
				sendItem = myIdentity.toString();	
			}
			Identity anotherTrain = radio.getReceivedIdentity();
			if(anotherTrain!=null) {
				receiveItem = anotherTrain.toString();
				addTrain(anotherTrain);
			} else
				receiveItem = "";
			for(int i = 0;i<anotherTrains.size();i++) {
				Identity train = anotherTrains.get(i);
				sameId = RuteUtil.getSameCoordIdWhere(myIdentity,train, false);
				timeToStop = timeLeftToStop(myIdentity.getSpeed(true)*3600.0/1000.0, myIdentity.getCurCoord());
				listCoordId = RuteUtil.getSameCoordIdWhere(myIdentity,train, true);
				message = isDangerous(train);
			}
		} catch (IOException e) {
			e.printStackTrace();
		}
	}

	private Message isDangerous(Identity anotherTrain) {
		if(isIntersected()){
			atFront = myIdentity.isAtFront(anotherTrain, sameId);
			inSameDirection = myIdentity.isInSameDirection(anotherTrain, sameId);
			distance = LengthMeasurer.distanceBetween(myIdentity, anotherTrain, sameId, isInSameDirection());
			LatLon myCurCoord = myIdentity.getCurCoord();
			LatLon anotherCurCoord = anotherTrain.getCurCoord();
			if(isInSameDirection()){
				if(isAtFront()){
					if(distance-Converter.meterToDegrees(myIdentity.getPanjangRangkaian(), myCurCoord)<Converter.meterToDegrees(1, myCurCoord)){
						collide = true;
					}
				}else{
					if(distance-Converter.meterToDegrees(anotherTrain.getPanjangRangkaian(),anotherCurCoord)<Converter.meterToDegrees(1, anotherCurCoord)){
						collide = true;
					}
				}
			}else{
				if(distance < Converter.meterToDegrees(1, myCurCoord)){
					collide = true;
				}
			}
			boolean isInSameflatform1 = myIdentity.isInSameFlatform(listCoordId);
			boolean isInSameflatform2 = anotherTrain.isInSameFlatform(listCoordId);
			//jika berlawanan arah
			if(!isInSameDirection()){
				if(!isInSameflatform1){
					if(!isInSameflatform2){
						// kereta lain sedang bergerak
						if(anotherTrain.getSpeed(false)>0){
							if(myIdentity.getSpeed(false)==0){
								return new Message(Message.WARNING, Message.WAIT,0);
							}else if(myIdentity.getSpeed(false)>0){
								if(anotherTrain.getMaxSpeed()>=myIdentity.getMaxSpeed()){
									return new Message(Message.WARNING, Message.BRAKE, 0);
								}else{
									return getSafeMessage();
								}
							}
							//kereta lain sedang berhenti
						}else{
							return getSafeMessage();
						}
					}else{
						Integer[] wesel = RuteUtil.getSameWeselId(myIdentity, anotherTrain, sameId);
						if(wesel.length==1 |wesel.length==2){
							if(myIdentity.getSpeed(false)==0){
								return new Message(Message.WARNING, Message.WAIT,0);
							}else if(myIdentity.getSpeed(false)>0){
								return new Message(Message.WARNING,Message.BRAKE,0);
							}
						}else {
							return getSafeMessage();
						}
					}
				}else{
					Integer[] wesel = RuteUtil.getSameWeselId(myIdentity, anotherTrain, sameId);
					if(isInSameflatform2){
						boolean isComplete = true;
						Integer[] urutan = myIdentity.getlistUrutan(listCoordId);
						int i = myIdentity.getUrutan();
						while(i<urutan.length && isComplete){
							if(!NumberUtil.isContains(urutan, i))
								isComplete = false;
							i++;
						}
						System.out.println("isComplete = "+isComplete +" wesel = "+wesel.length);
						if(wesel.length<=2&&isComplete){
							return headOnCollision(anotherTrain);
						}else{
							return getSafeMessage();
						}
					}else{
						return getSafeMessage();
					}
				}
				//Searah
			}else{
				System.out.println("same direction?" + isInSameDirection());
				if(!isInSameflatform1&&!isInSameflatform2){
					return rearEndFlanking(anotherTrain);
				}else{
					return rearEndCollision(anotherTrain);
				}
			}
		}
		return getSafeMessage();
	}

	private Message rearEndFlanking(Identity anotherTrain) {
		KoordinatBean firstWesel = RuteUtil.getFirstNextWesel(myIdentity);
		if(firstWesel.equals(RuteUtil.getFirstNextWesel(anotherTrain))){
			double timeleftToWesel1 = timeLeftTo(myIdentity, firstWesel.getId());
			double timeleftToWesel2 = timeLeftTo(anotherTrain, firstWesel.getId());
			double timeToStop1 = timeLeftToStop(myIdentity.getSpeed(false),myIdentity.getCurCoord());
			double timeToStop2 = timeLeftToStop(anotherTrain.getSpeed(false),anotherTrain.getCurCoord());
			if(timeToStop1*2.0>=timeleftToWesel1){
				if(timeToStop2*2.0>=timeleftToWesel2){
					if(timeleftToWesel1<timeleftToWesel2){
						return getSafeMessage();
					} else{
						return new Message(Message.DANGER, Message.BRAKE, 0);
					}
				}else if(timeToStop2*4.0>=timeleftToWesel2){
					if(timeleftToWesel1<timeleftToWesel2){
						return getSafeMessage();
					} else{
						return new Message(Message.WARNING, Message.BRAKE, 0);
					}
				}else{
					return getSafeMessage();
				}
			}else if(timeToStop1*4.0>=timeleftToWesel1 & timeToStop2*2.0>=timeleftToWesel2){
				if(timeleftToWesel1<timeleftToWesel2){
					return getSafeMessage();
				} else{
					return new Message(Message.WARNING, Message.BRAKE, 0);
				}
			}else{
				return getSafeMessage();
			}
		}else{
			return getSafeMessage();
		}
	}

	private Message headOnCollision(Identity anotherTrain){
		double timeToCrash = timeLeftToCrashHeadOn(anotherTrain);
		timeToSafeDistance = timeToCrash;
		double timeToStop1 = timeLeftToStop(myIdentity.getSpeed(false),myIdentity.getCurCoord());
		double timeToStop2 = timeLeftToStop(anotherTrain.getSpeed(false),anotherTrain.getCurCoord());

		System.out.println("timeToCrash = "+ timeToCrash);
		System.out.println("timeToStop1 = "+ timeToStop1);
		System.out.println("timeToStop2 = "+ timeToStop2);

		if((timeToStop1+timeToStop2)>=2.0*timeToCrash){
			System.out.println("(timeToStop1+timeToStop2)>=2.0*timeToCrash");
			System.out.println(new Message(Message.DANGER, Message.BRAKE, 0));
			return new Message(Message.DANGER, Message.BRAKE, 0);
		}else if((timeToStop1+timeToStop2)>=timeToCrash){
			System.out.println("(timeToStop1+timeToStop2)>=timeToCrash");
			System.out.println(new Message(Message.WARNING, Message.BRAKE, 0));
			return new Message(Message.WARNING, Message.BRAKE, 0);
		}else{
			return getSafeMessage();
		}
	}
	private Message rearEndCollision(Identity anotherTrain){
		double timeToCrash = timeLeftToCrashRearEnd(anotherTrain);
		timeToSafeDistance = timeToCrash;
		double timeLeftToDestSpeed = 0;
		if(!isAtFront()){
			timeLeftToDestSpeed = timeLeftUntilDestSpeed(myIdentity.getSpeed(false), anotherTrain.getSpeed(false),myIdentity.getCurCoord());
			if(timeLeftToDestSpeed>=timeToCrash){
				return new Message(Message.DANGER, Message.BRAKE, anotherTrain.getSpeed(true));
			}else if(timeLeftToDestSpeed>=timeToCrash*0.5){
				return new Message(Message.WARNING, Message.BRAKE, anotherTrain.getSpeed(true));
			}else{
				return getSafeMessage();
			}
		}else{
			timeLeftToDestSpeed = timeLeftUntilDestSpeed(anotherTrain.getSpeed(false), myIdentity.getSpeed(false),myIdentity.getCurCoord());
			if(timeLeftToDestSpeed>=timeToCrash){
				return getMessage(Message.DANGER);
			}else if(timeLeftToDestSpeed>=timeToCrash*0.5){
				return getMessage(Message.WARNING);
			}else{
				return getSafeMessage();
			}
		}
	}
	private Message getSafeMessage(){
		return getMessage(Message.SAFE);
	}
	private Message getMessage(int status){
		double min = Math.min(myIdentity.getAllowedMaxSpeed(), myIdentity.getMaxSpeed());
		if((min*1000.0/3600.0)>myIdentity.getSpeed(true)){
			return new Message(status, Message.GO,min);
		}else{
			return new Message(Message.WARNING, Message.BRAKE,min);
		}
	}
	private double timeLeftToCrashHeadOn(Identity anotherTrain){
		double myTrainSpeed = myIdentity.getSpeed(false);
		double anotherTrainSpeed = anotherTrain.getSpeed(false);
		double trainDistance = LengthMeasurer.distanceBetween(myIdentity,anotherTrain, sameId, isInSameDirection());
		return (trainDistance-Converter.meterToDegrees(500.0, myIdentity.getCurCoord()))/(myTrainSpeed+anotherTrainSpeed);
	}
	private double timeLeftToCrashRearEnd(Identity anotherTrain){
		Identity s1 = null;
		Identity s2 = null;
		if(isAtFront()){
			s2 = myIdentity;
			s1 = anotherTrain;
		}else{
			s1 = myIdentity;
			s2 = anotherTrain;
		}
		double panjangRangkaian = Converter.meterToDegrees(s2.getPanjangRangkaian(), s2.getCurCoord());
		double jarakAman = Converter.meterToDegrees(500.0, s2.getCurCoord());
		double jarakKereta =  LengthMeasurer.distanceBetween(myIdentity,anotherTrain, sameId, isInSameDirection());
		return (panjangRangkaian + jarakAman - jarakKereta)/(s2.getSpeed(false)-s1.getSpeed(false));
	}

	private double timeLeftTo(Identity train, Integer coordId){
		double distance = LengthMeasurer.distanceBetween(train, coordId);
		double speed = train.getSpeed(false);
		return distance/speed;
	}

	public double timeLeftToStop(double curSpeed, LatLon curCoord){
		return timeLeftUntilDestSpeed(curSpeed, 0, curCoord);
	}
	public double timeLeftUntilDestSpeed(double curSpeed, double destSpeed, LatLon curCoord){
		double acc = destSpeed>curSpeed?0.8:-0.8;
		return (destSpeed-curSpeed)/(Converter.meterToDegrees(acc, curCoord));
	}

	private boolean isIntersected(){
		if(sameId.length==0)
			return false;
		return true;
	}

	public void addTrain(Identity newTrain){
		int i = 0;
		boolean isExist = false;
		while(i<anotherTrains.size()&& !isExist){
			Identity aTrain = anotherTrains.get(i);
			if(aTrain.getKeretaApi().equals(newTrain.getKeretaApi())){
				isExist = true;
			}
			i++;
		}
		if(isExist){
			anotherTrains.get(i-1).setCurCoord(newTrain.getCurCoord());
			anotherTrains.get(i-1).setUrutan(newTrain.getUrutan());
		}else{
			anotherTrains.add(newTrain);
			trainQuantityChanged();	
		}
	}

	public void removeTrain(Identity trainToDelete){
		for(int i = 0;i<anotherTrains.size();i++){
			Identity train = anotherTrains.get(i);
			if(train.getKeretaApi().equals(trainToDelete.getKeretaApi()))
				anotherTrains.remove(i);
		}
		trainQuantityChanged();
	}

	private void trainQuantityChanged(){
		setChanged();
		notifyObservers();
	}

	public Message getMessage(){
		return message;
	}
	public boolean isAtFront(){
		return atFront;
	}
	public boolean isInSameDirection(){
		return inSameDirection;
	}
	public List<Identity> getAnotherTrains(){
		return anotherTrains;
	}
	public Identity getMyIdentity(){
		return myIdentity;
	}
	public boolean isCollide(){
		return collide;
	}

	public GPSSimulator getGpsSimulator() {
		return gpsSimulator;
	}

	public void setGpsSimulator(GPSSimulator gpsSimulator) {
		this.gpsSimulator = gpsSimulator;
	}

	public void stop() {
		gpsSimulator.stop();
	}

}
